#include "BlendedSteering.h"
#include "SteeringOutput.h"
#include "Agent.h"
#include "Error.h"

namespace Engine
{
	void BlendedSteering::Update(SteeringOutput* output)
	{
		SteeringOutput temp;

		for(auto it = m_behaviours.begin(); it != m_behaviours.end(); ++it)
		{
			it ->first ->Update(&temp);
			output ->linear += temp.linear * it ->second;
			output ->angular += temp.angular * it ->second;

			temp.Reset();
		}
		
		if(output ->linear.SquaredLength() > m_parent ->GetMaxSpeedSquared())
		{
			output ->linear.Normalize();
			output ->linear *= m_parent ->GetMaxSpeed();
		}
		
		output ->angular = output ->angular > m_parent ->GetMaxAngularAcc() ? 
			               m_parent ->GetMaxAngularAcc() : output ->angular;
	}

	void BlendedSteering::Draw()
	{
		for(auto it = m_behaviours.begin(); it != m_behaviours.end(); ++it)
		{
			it ->first ->Draw();
		}
	}

	void BlendedSteering::SetParent(Agent* parent)
	{
		m_parent = parent;
		
		for(auto it = m_behaviours.begin(); it != m_behaviours.end(); ++it)
			it ->first ->SetParent(parent);
	}

	void BlendedSteering::SetTarget(Agent* target)
	{
		m_target = target;

		for(auto it = m_behaviours.begin(); it != m_behaviours.end(); ++it)
			it ->first ->SetParent(target);
	}
}